#include <ros/ros.h>
#include <grid_map_ros/grid_map_ros.hpp>

using namespace grid_map;

int main(int argc, char **argv)
{
    ros::init(argc, argv, "grid_map_tutorial_demo");
    ros::NodeHandle nh("~");
    ros::Publisher publisher = nh.advertise<grid_map_msgs::GridMap>("grid_map", 1, true);

    GridMap map({"elevation"});
    map.setFrameId("map");
    map.setGeometry(Length(2.0, 2.0), 0.001, Position(0.0, -0.1));

    ros::Rate rate(30.0);
    while (nh.ok())
    {

        // Add data to grid map.
        ros::Time time = ros::Time::now();
        for (GridMapIterator it(map); !it.isPastEnd(); ++it)
        {
            Position position;
            map.getPosition(*it, position);
            map.at("elevation", *it) = 0.1 * position.x() - 0.1 * position.y();
        }

        // Publish grid map.
        map.setTimestamp(time.toNSec());
        grid_map_msgs::GridMap message;
        GridMapRosConverter::toMessage(map, message);
        publisher.publish(message);
        ROS_INFO_THROTTLE(1.0, "Grid map (timestamp %f) published.", message.info.header.stamp.toSec());

        // Wait for next cycle.
        rate.sleep();
    }

    return 0;
}